#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <iomanip>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>

const double R2D = 180.0 / 3.1415926535;
const double D2R = 3.1415926535 / 180.0;

/**
 * @brief 数据类型转换联合体，用于float和字节数组间的转换
 */
typedef union
{
	float data;
	unsigned char data8[4];
} data_u;

int main(int argc, char** argv)
{
	ros::init(argc, argv, "read_imu_odom");
	ros::NodeHandle nh;
	ros::NodeHandle private_nh("~");

	time_t tt = time(NULL);
    struct tm* t = localtime(&tt);

    std::stringstream filename;
    filename << "/home/nano/workspace/ai-robot/catkin_ws/" << "odom_" << t->tm_year + 1900 << "_"
                                                                    << t->tm_mon + 1 << "_" 
                                                                    << t->tm_mday << "_" 
                                                                    << t->tm_hour << "_" 
                                                                    << t->tm_min << "_" 
                                                                    << t->tm_sec << ".txt";
    std::ofstream output_file_s;
    output_file_s.open(filename.str().c_str());
    output_file_s.precision(15);

    auto startTime = ros::Time::now();

	bool is_auto_start = false;
	private_nh.param("is_auto_start", is_auto_start, false);

	// 如果要让该节点开机自启，则需要延时等待串口得到权限
	if(is_auto_start)
		ros::Duration(10).sleep();

	serial::Serial ser;
	std::string serial_port;
	int serial_baudrate;
	private_nh.param("serial_port", serial_port, std::string("/dev/ttyACM0"));
	private_nh.param("serial_baudrate", serial_baudrate, 115200);
	// 打开串口
	try
	{
		ser.setPort(serial_port);
		ser.setBaudrate(serial_baudrate);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port ");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port" );

	// 清空串口缓存区
	ser.read(ser.available());

	// 底盘复位
	uint8_t _cnt = 0;
	uint8_t data_to_send[100]; // 待发送的字节数组
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x55;
	uint8_t _start = _cnt;
	data_to_send[_cnt++]=0x11;
	data_to_send[_cnt++]=0x22;
	data_to_send[_cnt++]=0x33;
	data_to_send[_cnt++]=0x44;
	data_to_send[_cnt++]=0x55;
	data_to_send[_cnt++]=0x66;
	data_to_send[_cnt++]=0x77;
	data_to_send[_cnt++]=0x88;
	data_to_send[_cnt++]=0x99;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xBB;
	data_to_send[_cnt++]=0xCC;
    char checkout = 0;
    for(int i = _start; i < _cnt; i++)
    {
        checkout += data_to_send[i];
    }
	data_to_send[_cnt++] = checkout;

    // 串口发送
    ser.write(data_to_send, _cnt);
    // 等待底盘复位
	ros::Duration(1.0).sleep();
	ser.read(ser.available());

	float x 	= 0.0;	// x轴位置
	float y 	= 0.0;	// y轴位置

	ros::Time current_time, last_time;
	current_time = ros::Time::now();
 	last_time = ros::Time::now();

	// 设置读取频率，读取频率 > 数据包长度 * 上传频率
	ros::Rate loop_rate(1000000); 

	int state = 0;
	uint8_t r_buffer[1024] = {0};
	unsigned char data_receive[21] = {0};
    int data_cnt = 0;
    while(ros::ok())
	{
		if(ser.available())
		{
            uint8_t buffer = 0;
			ser.read(&buffer, 1);

			if(state == 0 && buffer == 0xAA)
			{
				current_time = ros::Time::now();
				state++;
			}
			else if(state == 1 && buffer == 0x55)
			{
				state++;
			}
			else if(state == 2)
			{
				data_receive[data_cnt++]=buffer;
				if(data_cnt == 21)
				{
					
					/* 进行数据校验 */
					uint8_t checkout = 0;
					for(int k = 0; k < data_cnt - 1; k++)
					{
						checkout += data_receive[k];
					}
					if(checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一个字节是校验码 
					{
						/* 校验通过，进行解码 */
						float vx, vy, vth, th; // x轴线速度，y轴线速度，z轴角速度，偏航角
						float* datas_ptr[] = {&vx, &vy};
						data_u temp;
						for(int i = 0; i < sizeof(datas_ptr) / sizeof(float*); i++)
						{
							temp.data8[0] = data_receive[4 * i + 0 + 12];
							temp.data8[1] = data_receive[4 * i + 1 + 12];
							temp.data8[2] = data_receive[4 * i + 2 + 12];
							temp.data8[3] = data_receive[4 * i + 3 + 12];							
							*(datas_ptr[i]) = temp.data;
						}
						

						float gyro_rps[3];
						float accel_mpss[3];
						for(int i = 0; i < 3; i++){
							int16_t gyro = *((int16_t*)data_receive + i);
							int16_t accel = *((int16_t*)data_receive + i + 3);
							gyro_rps[i] = gyro * 0.0010650782;
							accel_mpss[i] = accel * 0.004790;
							
						} 

						output_file_s   << (current_time - startTime).toSec() << " "
										<< gyro_rps[0] << " " << gyro_rps[1] << " " << gyro_rps[2] << " " 
										<< accel_mpss[0] << " " << accel_mpss[1] << " " << accel_mpss[2] << " "
										<< vx << " " 
										<< vy << " " << std::endl;

						ROS_INFO("linear_x: %f, linear_y: %f, angular_z: %f", accel_mpss[0], accel_mpss[1], accel_mpss[2]);
						// ROS_INFO("linear_x: %f, linear_y: %f, angular_z: %f", gyro_rps[0], gyro_rps[1], gyro_rps[2]);
						// ROS_INFO("linear_x: %f, linear_y: %f", vx, vy);
						
					}
					
					data_cnt = 0;
					state = 0;
				}
			}
			else state = 0;
		}
		loop_rate.sleep();
	}
	// 关闭串口
	ser.close();

	return 0; 
}